/**
* \file
* \brief Peggy2 I2C interface - reads streams of data over I2C and displays content on Peggy2 LED pegboard
*
* This code is designed for an unmodified Peggy 2.0 board sold by evilmadscience.com
* The code configures the Peggy as an TWI (I2C) slave.
* Companion code for an Arduino allows it to act as an TWI master, so that it can transmit frames to the peggy.
* Core I2C and display code are exact duplicates of Jay Clegg's original code (see credits below).
* My additions are rather in expanding the usage of "built-in" graphical functions that allows putting more of
* the work in creating graphical effects on the shoulders of Peggy, rather than on the external sender (in my
* case called "MegaMan" in this project. This is done to offload MegaMan, who is also supposed to play MP3 files,
* communicate with the last member of the team ("Wally - the sensor turret), report status over serial and XBee
* to a PC and a log server respectively. I did that through using the expanded header the Jay had made but left
* unused as well as introduce one more state (6 in stead of Jay's original 5). I also use the flash of Peggy to
* store some pictures used in the animations.
*
* Credits goes to:
*	Jay Clegg (http://www.planetclegg.com/projects/Twi2Peggy.html)
*		for creating the original program that I have expanded. In reality, he deserves 90% of
*		the credit for this program as the core components of this code is unchanged from
*		Jay's original. If you want to do something similar to what I have been
*		doing here, I really recommend starting with his version of the code, which is smaller,
*		neater and probably a better starting point to understand what is going on.
* 		Please see http://www.planetclegg.com/projects/Twi2Peggy.html for explanation of how all this is supposed to work.
*	Windell H Oskay, (http://www.evilmadscientist.com/)
*		for creating the Peggy 2.0 kit, and getting 16 shades of gray working
*	Geoff Harrison (http://www.solivant.com/peggy2/),
*		for proving that interrupt driven display on the Peggy 2.0 was viable.
*
* \addtogroup Peggy
* \{
*
* \copyright Copyright 2012 Ole Jakob Skjelten.  All rights reserved.
*
*    This program is free software: you can redistribute it and/or modify
*    it under the terms of the GNU General Public License as published by
*    the Free Software Foundation, either version 3 of the License, or
*    (at your option) any later version.
*
*    This program is distributed in the hope that it will be useful,
*    but WITHOUT ANY WARRANTY; without even the implied warranty of
*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*    GNU General Public License for more details.
*
*    You should have received a copy of the GNU General Public License
*    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <util/delay.h>	// OJ need this for delay() which we use in animaton
#include <avr/pgmspace.h> // OJ need this for pictures stored in FLASH to save RAM

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// FPS must be high enough to not have obvious flicker, low enough that main loop has time to process one byte per pass.
// ~140 seems to be about the absolute max for me (with this code on avr-gcc 4.2, -Os), but compiler differences might
// make this maximum value larger or smaller. If the value is too high errors start to occur or it will stop receiving
// altogether, conversely, any lower than 60 and flicker becomes apparent.
// note: further code optimization might allow this number to be a bit higher, but only up to a point...
// it *must* result in a value for OCR0A in the range of 1-255

#define FPS 80 					///< OJ: Trying a bit lower than default 100 to give us more time to do other stuff

#define DISP_BUFFER_SIZE 325	///< 25 rows * 13 bytes per row == 325 (13 bytes due to 4bit per pixel => 12.5B)
#define MAX_BRIGHTNESS 15		///< More than 4bits color depth doesn't really show on the display

#define TWI_SLAVE_ID 34			///< The I2C slave address


// I2C Slave Receiver staus codes, from Atmel notes
#define TWI_SRX_ADR_ACK            0x60
#define TWI_SRX_ADR_ACK_M_ARB_LOST 0x68
#define TWI_SRX_GEN_ACK            0x70
#define TWI_SRX_GEN_ACK_M_ARB_LOST 0x78
#define TWI_SRX_ADR_DATA_ACK       0x80
#define TWI_SRX_ADR_DATA_NACK      0x88
#define TWI_SRX_GEN_DATA_ACK       0x90
#define TWI_SRX_GEN_DATA_NACK      0x98
#define TWI_SRX_STOP_RESTART       0xA0
#define TWI_NO_STATE               0xF8
#define TWI_BUS_ERROR              0x00

////////////////////////////////////////////////////////////////////////////////////////////
uint8_t frameBuffer[DISP_BUFFER_SIZE];

uint8_t *currentRowPtr = frameBuffer;
uint8_t currentRow=0;
uint8_t currentBrightness=0;

volatile boolean abortFlag = false;

// A couple of global PROGMEM variables used for images for the animations...

PROGMEM prog_uchar skull[325] = {	///< Skull image, used for laughing skull animation
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,  15,   0,   0,   0,   0,   0,   0,  15,   0,   0,   0,
	  0,   0, 243, 240,   0,   0,   0,   0,   0, 240, 240,   0,   0,
	  0,   0, 241,  63,   0,   0,   0,   0,  15,   0, 240,   0,   0,
	  0, 255,  17, 240,   0,   0,   0,   0,   0, 241,  31, 240,   0,
	 15,   0,   1,  63,   0, 255, 255, 240,  15,  17,  17,  63,   0,
	  0, 240, 241,  19, 255,   0,   0,  13, 209,  19, 243, 240,   0,
	  0,  15,  15,  31,   0,   0,   0,   0,  14,  63,  15,   0,   0,
	  0,   0,   0, 240,  15,   0,   0,  15,   1, 224,   0,   0,   0,
	  0,   0,   0, 240, 240, 240,   0, 240, 241, 240,   0,   0,   0,
	  0,   0,   0, 240, 240, 255, 255, 240, 241, 208,   0,   0,   0,
	  0,   0,  15, 208, 240, 255,  15, 240, 241, 223,   0,   0,   0,
	  0,   0,  15,   0, 240,   0, 240,   0, 241,  31,   0,   0,   0,
	  0,   0,  15,   0,  15, 255,  15, 255,  17,  31,   0,   0,   0,
	  0,   0,   0, 240,   0,   1,  17,  17,  17, 240,   0,   0,   0,
	  0,   0,   0,  15, 223,  17,  17,  31, 255,   0,   0,   0,   0,
	  0,   0,   0, 243, 211, 223, 223, 211, 241, 240,   0,   0,   0,
	  0,  15,  15,  49, 209, 241, 241, 241, 241,  31,  15,   0,   0,
	  0, 243, 243,  17, 255,  17,  17,  31, 243,  17, 240, 240,   0,
	 15,  49,  17,  31,  15, 255, 255, 255,  15,  49,  16,  15,   0,
	  0, 255,  17, 240,   0,   0,   0,   0,   0, 241,  31, 240,   0,
	  0,   0, 240,  15,   0,   0,   0,   0,  15,  49, 240,   0,   0,
	  0,   0, 240, 240,   0,   0,   0,   0,   0, 243, 240,   0,   0,
	  0,   0,  15,   0,   0,   0,   0,   0,   0,  15,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0
	};

PROGMEM prog_uchar radar[325] = {	///< Radar image, used for radar scan animation
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0, 255, 255, 240,   0,   0,   0,   0,   0,
	  0,   0,   0,  15, 255, 240,   0, 255, 255,   0,   0,   0,   0,
	  0,   0,  15, 240,   0,   0,   0,   0,   0, 255,   0,   0,   0,
	  0,  15, 240,   0,   0,   0,   0,   0,   0,   0, 255,   0,   0,
	  0, 240,   0,   0,   0,   0,   0,   0,   0,   0,   0, 240,   0,
	 15,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  15,   0,
	240,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0, 240,
	 15,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,  15,   0,
	  0, 240,   0,   0,   0,   0,   0,   0,   0,   0,   0, 240,   0,
	  0,  15,   0,   0,   0,   0,   0,   0,   0,   0,  15,   0,   0,
	  0,   0, 240,   0,   0,   0,   0,   0,   0,   0, 240,   0,   0,
	  0,   0,  15,   0,   0,   0,   0,   0,   0,  15,   0,   0,   0,
	  0,   0,   0, 240,   0,   0,   0,   0,   0, 240,   0,   0,   0,
	  0,   0,   0,  15,   0,   0,   0,   0,  15,   0,   0,   0,   0,
	  0,   0,   0,   0, 240,   0,   0,   0, 240,   0,   0,   0,   0,
	  0,   0,   0,   0,  15,   0,   0,  15,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0, 240,   0, 240,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,  15,  15,   0,   0,   0,   0,   0,   0,
	  0,   0,   0,   0,   0,   0, 240,   0,   0,   0,   0,   0,   0
	};

/**
*	\brief Timer interrupt function to redraw screen from data in frameBuffer (13*24)B
*
*/
ISR (TIMER2_COMPA_vect)
{
	//if ( (TWCR & (1<<TWINT)) && (TWSR == TWI_SRX_ADR_DATA_ACK) ) abortFlag = true;
	//if (TWCR & (1<<TWINT))
	//	if ( (TWSR == TWI_SRX_GEN_ACK) || (TWSR == TWI_SRX_ADR_ACK) ) abortFlag = true;

	//if (TWCR & (1<<TWINT)) abortFlag = true;
	//if (TWDR == 0xde) abortFlag = true;

	/*
	*	There are 15 passes through this interrupt for each row per frame - ( 15 * 25) = 375 times per frame.
	*	during those 15 passes, a led can be on or off.
	*	if it is off the entire time, the perceived brightness is 0/15
	*	if it is on the entire time, the perceived brightness is 15/15
	*	giving a total of 16 average brightness levels from fully on to fully off.
	*	currentBrightness is a comparison variable, used to determine if a certain
	*	pixel is on or off during one of those 15 cycles.
	*	currentBrightnessShifted is the same value left shifted 4 bits:  This is just an optimization for
	*	comparing the high-order bytes.
	*/
	if (++currentBrightness >= MAX_BRIGHTNESS)
	{
		currentBrightness=0;
		if (++currentRow > 24)
		{
			currentRow = 0;
			currentRowPtr = frameBuffer;
		}
		else
		{
			currentRowPtr += 13;
		}
	}


	////////////////////  Parse a row of data and write out the bits via SPI
	uint8_t currentBrightnessShifted = currentBrightness <<4;

	uint8_t *ptr = currentRowPtr + 12;  // its more convenient to work from right to left
	uint8_t p, bits=0;

	// optimization: by using variables for these two masking constants, we can trick gcc into not
	// promoting to 16-bit int (constants are 16 bit by default, causing the
	// comparisons to get promoted to 16bit otherwise)].  This turns out to be a pretty
	// substantial optimization for this handler
	uint8_t himask = 0x0f;
	uint8_t lomask = 0xf0;

	// Opimization: interleave waiting for SPI with other code, so the CPU can do something useful
	// when waiting for each SPI transmission to complete

	p = *ptr--;
	if ((p & lomask) > currentBrightnessShifted)  			bits|=1;
	SPDR = bits;

	bits=0;
	p = *ptr--;
	if ((p & lomask) > currentBrightnessShifted)  			bits|=64;
	if ((p & himask) > currentBrightness)	bits|=128;
	p = *ptr--;
	if ((p & lomask) > currentBrightnessShifted)  			bits|=16;
	if ((p & himask) > currentBrightness)	bits|=32;
	p = *ptr--;
	if ((p & lomask) > currentBrightnessShifted)  			bits|=4;
	if ((p & himask) > currentBrightness)	bits|=8;
	p = *ptr--;
	if ((p & lomask) > currentBrightnessShifted)  			bits|=1;
	if ((p & himask) > currentBrightness)	bits|=2;

	while (!(SPSR & (1<<SPIF)))  { } // wait for prior bitshift to complete
	SPDR = bits;


	bits=0;
	p = *ptr--;
	if ((p & lomask) > currentBrightnessShifted)  			bits|=64;
	if ((p & himask) > currentBrightness)	bits|=128;
	p = *ptr--;
	if ((p & lomask) > currentBrightnessShifted)  			bits|=16;
	if ((p & himask) > currentBrightness)	bits|=32;
	p = *ptr--;
	if ((p & lomask) > currentBrightnessShifted)  			bits|=4;
	if ((p & himask) > currentBrightness)	bits|=8;
	p = *ptr--;
	if ((p & lomask) > currentBrightnessShifted)  			bits|=1;
	if ((p & himask) > currentBrightness)	bits|=2;

	while (!(SPSR & (1<<SPIF)))  { } // wait for prior bitshift to complete
	SPDR = bits;


	bits=0;
	p = *ptr--;
	if ((p & lomask) > currentBrightnessShifted)  			bits|=64;
	if ((p & himask) > currentBrightness)	bits|=128;
	p = *ptr--;
	if ((p & lomask) > currentBrightnessShifted)  			bits|=16;
	if ((p & himask) > currentBrightness)	bits|=32;
	p = *ptr--;
	if ((p & lomask) > currentBrightnessShifted)  			bits|=4;
	if ((p & himask) > currentBrightness)	bits|=8;
	p = *ptr--;
	if ((p & lomask) > currentBrightnessShifted)  			bits|=1;
	if ((p & himask) > currentBrightness)	bits|=2;

	while (!(SPSR & (1<<SPIF)))  { }// wait for prior bitshift to complete
	SPDR = bits;

	////////////////////  Now set the row and latch the bits

	uint8_t portD;


	if (currentRow < 15)
		portD = currentRow+1;
	else
		portD = (currentRow -14)<<4;


	while (!(SPSR & (1<<SPIF)))  { } // wait for last bitshift to complete

	//if (currentBrightness == 0)
	PORTD = 0;				// set all rows to off
	PORTB |= _BV(1);//(1<<PB1); //  latch it, values now set
	//if (currentBrightness == 0)
	PORTD = portD;     // set row
	PORTB &= ~( _BV(1));//~((1<<PB1)); // reset latch for next time

	// notes to self, calculations from the oscope:
	// need about minimum of 6us total to clock out all 4 bytes
	// roughly 1.5ms per byte, although some of that is
	// idle time taken between bytes.  6=7us therefore is our
	// absolute minimum time needed to refresh a row, not counting calculation time.
	// Thats just if we do nothing else when writing out SPI and toggle to another row.
	//Measured values from this routine
	// @ 144 fps the latch is toggled every 19us with an actual 4byte clock out time of 12-13us
	// @ 70 fps the latch is toggle every 39us, with a clock out time of 13-14us
	// times do not count setup/teardown of stack frame

	// one byte @ 115k takes 86us (max) 78us (min) , measured time
	// one byte @ 230k takes 43us (max) 39us (min) , measured time
	// so 230k serial might barely be possible, but not with a 16mhz crystal (error rate to high)
	// 250k might just barely be possible
}

/*
ISR(TIMER0_COMPA_vect) {
	sei();
	if (TWCR & (1<<TWINT) && TWSR == 0x80) {
			frameBuffer[0] = 255;
			abortFlag = true;
			} else {
			frameBuffer[0] = 0;
			}
}*/

/*ISR (TWI_vect)
{
	sei();
	memset(frameBuffer, 0, sizeof(frameBuffer));
	while(1){};
	//if ( (TWSR == TWI_SRX_GEN_ACK) || (TWSR == TWI_SRX_ADR_ACK) ) abortFlag = true;
}*/


/**
*	\brief Initializes the display (sets up the SPI interface and interrupt)
*
*/
void displayInit(void)
{
	// need to set output for SPI clock, MOSI, SS and latch.  Eventhough SS is not connected,
	// it must apparently be set as output for hardware SPI to work.
	DDRB =  (1<<DDB5) | (1<<DDB3) | (1<<DDB2) | (1<<DDB1);
	// set all portd pins as output
	DDRD = 0xff;

	PORTD=0; // select no row

	// enable hardware SPI, set as master and clock rate of fck/2
	SPCR = (1<<SPE) | (1<<MSTR);
	SPSR = (1<<SPI2X);

	// setup the 2-interrupt.
	TCCR2A = (1<<WGM21);	// clear timer on compare match
	TCCR2B = (1<<CS21);		// timer uses main system clock with 1/8 prescale
	OCR2A  = (F_CPU >> 3) / 25 / 15 / FPS; // Frames per second * 15 passes for brightness * 25 rows
	TIMSK2 = (1<<OCIE2A);	// call interrupt on output compare match

/*
	// setup the 0-interrupt.

	TCCR0A = (1<<WGM01);	// clear timer on compare match
	TCCR0B = (1<<CS01);		// timer uses main system clock with 1/8 prescale
	OCR0A  = (F_CPU >> 3) / 111111; // Every 10th of a second
	TIMSK0 = (1<<OCIE0A);	// call interrupt on output compare match
*/

	for (uint8_t i=0; i < 4; i++)
	{
		SPDR = 0;
		while (!bit_is_set(SPSR, SPIF)) {}
	}
}


////////////////////////////////////////////////////////////////////////////////////////////
// I2C  routines
////////////////////////////////////////////////////////////////////////////////////////////



/**
*	\brief Initializes the I2C (2-wire/TWI) interface in slave receiver mode
*
*/
void initTwiSlave(uint8_t addr)
{

	PORTC |=  _BV(5) | _BV(4);	// enable pullups

	TWAR = (0<<TWGCE) |((uint8_t) (0xff & (addr<<1)));    // set slave address, no general call address
	TWDR = 0xff; // Default content = SDA released

	TWCR =	(1<<TWINT)	|	// "clear the flag"  (clear I2C interrupt)
			(1<<TWEA)	|	// send acks to master when getting address or data
			(0<<TWSTA)	|	// not a master, cant do start (don't generate start condition)
			(0<<TWSTO)	|	// Don't generate stop condition (doc says set these to 0)
          	(0<<TWWC)	|
			(1<<TWEN)	|	// hardware I2C enabled
			(0<<TWIE);		// disable I2C interrupts

	while (TWCR & (1<<TWIE)) { }	// wait for the I2C transceiver to get ready /

}

/**
*	\brief Returns a single byte from the I2C (2-wire/TWI) interface
*
*	\return Single byte value (uint8_t result) from the I2C bus
*/
uint8_t getTwiByte(void)
{
	uint8_t result=0;

  	keepListening:

 	// wait for an state change
  	//while (!(TWCR & (1<<TWINT))) { } // wait for TWINT to be set


  	//uint8_t sr = TWSR;
  	switch (TWSR)
  	{
	    case TWI_SRX_ADR_DATA_ACK:  // received a byte of data data
	    case TWI_SRX_GEN_DATA_ACK:
			result = TWDR;
	      	TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA);
			break;


//        case TWI_SRX_GEN_ACK_M_ARB_LOST:
//        case TWI_SRX_ADR_ACK_M_ARB_LOST:
    	case TWI_SRX_GEN_ACK:
    	case TWI_SRX_ADR_ACK:      // receive our address byte
		      TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA);
		      goto keepListening;
		      break;

	    case TWI_SRX_STOP_RESTART:       // A STOP or repeated START condition was received
	    	TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA);
		    goto keepListening;
	      	break;

	    case TWI_SRX_ADR_DATA_NACK:   // data received, returned nack
	    case TWI_SRX_GEN_DATA_NACK:
	      	TWCR = (1<<TWEN)|(1<<TWINT);
	      	goto keepListening;
			break;

    	case TWI_NO_STATE:
    		goto keepListening;
    		break;

	    default:     			 // something bad happened. assuming a bus error, we try to recover from this
	      // reset state and continue
	      TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO);    // ignore
		  while(TWCR & (1<<TWSTO)){ }	// wait for stop condition to be exectued; TWINT will not be set after a stop
		  result = 0xff;
	      break;
    }

	return result;
}

/**
*	\brief Runs commands given by header 0xdeadbeef0101x where x is command
*
*	This function moves some common animations and other effects over to the Peggy to save MegaMan from having to
*	transmit a lot of data over the I2C bus for often used effects.
*
*	\param uint8_t cmd Command to execute (1: clear screen, 2: skull animation, 3: radar sweep, 4: "busy" anim, 5: "sound wave" anim)
*/
void runCommand(uint8_t cmd) {
abortFlag = false;
	switch (cmd) {

		case 1:		// Clear screen (updated to use memset() rather than a for-loop
			memset(frameBuffer, 0, sizeof(frameBuffer));
			break;

		case 2: {	// skull animation
			uint8_t counter = 5;
		  while (counter--) {
			if(abortFlag) return;	// abort the runCommand function if new data is waiting on the I2C bus

			char skullAnim[] = {
			  15, 223, 223, 223, 223, 255,   0,
			  243, 211, 241, 241, 243, 241, 240,
			  49, 209,   0,   0,   1, 241,  31,
			  17, 255,  17,  17,  31, 243,  17,
			  31,  15, 241,  17, 255,  15,  49,
			  240,   0, 255, 255, 240,   0, 241,
			  15,   0,   0,   0,   0,  15,  49
			 };

			  // Get image from flash
			  for (uint16_t i = 0; i < 325; i++)
				  frameBuffer[i] = pgm_read_byte_near(&skull[0] + i);

			_delay_ms(100); // gives time to see original picture
			for (uint8_t x = 0; x < 7; x++)
				for (uint8_t y = 0; y < 7; y++) {
	 				if(abortFlag) return;	// abort the runCommand function if new data is waiting on the I2C bus
					frameBuffer[(3 + x) + ((15 + y) * 13)] = skullAnim[x + (y * 7)];
				}
			  _delay_ms(100); // gives time to see the open mouth animation
		  }	// end of while loop
		  break;
		} // end of case 2
		case 3: {	// scan animation
			memset(frameBuffer, 0, sizeof(frameBuffer));	// clearing screen before we begin
			uint8_t position = 0;
			boolean leftRight = 1;


			for (uint8_t iterations = 0; iterations < 24; iterations++) {	// 3 full sweeps (3*24=72)
				if(abortFlag == true) return;	// abort the runCommand function if new data is waiting on the I2C bus

				//				 0   1   2   3   4  5  6  7  8  9 10 11 12 13 14 15 16 17 18 19  20  21  22  23  24 ////////////////////////
				uint8_t yScan[] = { 13, 12, 11, 10, 10, 9, 9, 8, 8, 8, 8, 7, 7, 6, 7, 8 ,8, 8, 9, 9, 10, 10, 11, 12, 13 };

				// Get image from flash
				for (uint16_t i = 0; i < 325; i++)
					frameBuffer[i] = pgm_read_byte_near(&radar[0] + i);


				//drawLine(12,23,position,yScan[position], 15);	//main line
				//_delay_ms(3000);

				// drawing "shade"
				if (leftRight && position >= 1) 	drawLine(12,23,position-1,yScan[position-1], 12);	//main line
				if (leftRight && position >= 2) 	drawLine(12,23,position-2,yScan[position-2], 10);	//main line
				if (leftRight && position >= 3) 	drawLine(12,23,position-3,yScan[position-3], 7);	//main line
				if (!leftRight && position <= 23)	drawLine(12,23,position+1,yScan[position+1], 12);	//main line
				if (!leftRight && position <= 22) 	drawLine(12,23,position+2,yScan[position+2], 10);	//main line
				if (!leftRight && position <= 21) 	drawLine(12,23,position+3,yScan[position+3], 7);	//main line

				_delay_ms(10);

				if (leftRight) position++;
				if (!leftRight) position--;
				if (position == 24) leftRight = 0;
				if (position == 0) leftRight = 1;
			}
			memset(frameBuffer, 0, sizeof(frameBuffer));	// clearing screen when we're done

			break;
		} // end of case 3
		case 4: { // Animated "in progress" animation of star, we loop through different gradients. Loop gives one full rotation.
			memset(frameBuffer, 0, sizeof(frameBuffer));    // clear screen

			uint8_t gradients[16] = { 15, 10, 6, 1, 1, 1, 1, 1,        // we rotate through this array to get our animation
									15, 10, 6, 1, 1, 1, 1, 1};
			//uint8_t	gradients[] = { 5, 5, 5, 5, 5, 5, 5, 5,        // we rotate through this array to get our animation
			//				  		0, 0, 0, 0, 0, 0, 0, 0};

			uint8_t i, j;
			for (j = 0; j < 5; j++)
			for (i = 0; i < 8; i++) {
				if(abortFlag) return;	// abort the runCommand function if new data is waiting on the I2C bus
				int abc = 0;

				drawLine(2,1,10,9,gradients[i+0]);    drawLine(1,2,9,10,gradients[i+0]);
				_delay_us(abc);
				drawLine(12,1,12,10,gradients[i+1]);  drawLine(13,1,13,10,gradients[i+1]);
				_delay_us(abc);
				drawLine(23,1,15,9,gradients[i+2]);   drawLine(24,2,16,10,gradients[i+2]);
				_delay_us(abc);
				drawLine(15,12,24,12,gradients[i+3]); drawLine(15,13,24,13,gradients[i+3]);
				_delay_us(abc);
				drawLine(15,16,23,24,gradients[i+4]); drawLine(16,15,24,23,gradients[i+4]);
				_delay_us(abc);
				drawLine(12,15,12,24,gradients[i+5]); drawLine(13,15,13,24,gradients[i+5]);
				_delay_us(abc);
				drawLine(1,23,9,15,gradients[i+6]);   drawLine(2,24,10,16,gradients[i+6]);
				_delay_us(abc);
				drawLine(1,12,10,12,gradients[i+7]);  drawLine(1,13,10,13,gradients[i+7]);
				_delay_us(100);        // pause for breath
			}

			memset(frameBuffer, 0, sizeof(frameBuffer));    // clear screen
			break;
		} // end of case 4
		case 5: {
			memset(frameBuffer, 0, sizeof(frameBuffer));    // clear screen

			// Drawing our little "sound sensor" in the lower left corner of display
			setPixel(1,22,15);
			setPixel(2,22,15);
			setPixel(1,23,15);
			setPixel(2,23,15);

			setPixel(1,20,15);
			setPixel(2,21,15);
			setPixel(3,22,15);
			setPixel(4,23,15);

			setPixel(1,19,8);
			setPixel(2,19,8);
			setPixel(2,20,8);
			setPixel(3,21,8);
			setPixel(4,22,8);
			setPixel(5,22,8);
			setPixel(5,23,8);

			uint8_t r;

			for (r = 0; r < 50; r++) {	// enough to get three waves all the way outside the screen
			if(abortFlag) return;	// abort the runCommand function if new data is waiting on the I2C bus
				_delay_us(5000); // NOTE: needs adjustment
				if (r< 30) soundWave(4, 20, r, 15);
				if (r-1< 30) if (r>0) soundWave(4, 20, r-1, 12);
				if (r-2< 30) if (r>1) soundWave(4, 20, r-2, 9);
				if (r-3< 30) if (r>2) soundWave(4, 20, r-3, 0);

				if (r>6) {
					if (r-7< 36) if (r>6) soundWave(4, 20, r-7, 15);
					if (r-8< 36) if (r>7) soundWave(4, 20, r-8, 12);
					if (r-9< 36) if (r>8) soundWave(4, 20, r-9, 9);
					if (r-10< 36) if (r>9) soundWave(4, 20, r-10, 0);
				}

				if (r>6) {
					if (r-14< 42) if (r>14) soundWave(4, 20, r-14, 15);
					if (r-15< 42) if (r>15) soundWave(4, 20, r-15, 12);
					if (r-16< 42) if (r>16) soundWave(4, 20, r-16, 9);
					if (r-17< 42) if (r>17) soundWave(4, 20, r-17, 0);
				}
			}

			_delay_us(5000); // NOTE: needs adjustment
			memset(frameBuffer, 0, sizeof(frameBuffer));    // clear screen

		} // end of case 5


	} // end of switch
} // end of function

/**
*	\brief Puts a single pixel in the display
*
*	Updates the frameBuffer directly for individual pixels
*	Used by higher level functions, like drawLine()
*	Will ignore pixels outside of the 0 < x < 25 and 0 < y < 25 range
*
*	\param uint8_t x x-position (0-24)
*	\param uint8_t y y-position (0-24)
*	\param uint8_t gradient pixel color (0-15)
*/
void setPixel(uint8_t x, uint8_t y, uint8_t gradient) {

	if ( (x> 24) || (y > 24) || (x <0) || (y <0)) return; // we simply ignore pixels outside the visible screen

	uint16_t pos;
    if (x % 2 == 0) {								// We have an *even* x coordinate for the pixel - i.e. leftmost in the byte (MSB)
     	pos = (x / 2) + (y * 13);					// Convert pixel pos to byte number (for the row)
     	frameBuffer[pos] = ((frameBuffer[pos] & 0x0F)| ((gradient << 4) & 0xF0));
    } else {				 						// We have an odd number of pixel
		x--;										// We subtract one to get the right nibble address (beginning of the byte we need)
		pos = (x / 2) + (y * 13);					// Convert pixel pos to byte number (for the row)
		frameBuffer[pos] = ((frameBuffer[pos] & 0xF0) | (gradient & 0x0F));
    }
}

/**
*	\brief Draws a line on the display
*
*	Updates the frameBuffer through setPixel()
*
*	\param uint8_t xa horizontal start position (0-24)
*	\param uint8_t ya vertical start position (0-24)
*	\param uint8_t xb horizontal end position (0-24)
*	\param uint8_t yb vertical end position (0-24)
*	\param uint8_t gradient pixel color (0-15)
*/
void drawLine(uint8_t xa, uint8_t ya, uint8_t xb, uint8_t yb, uint8_t gradient){
	int8_t dx, dy, steps, k;	// note that using uint8_t here will not work as dx and dy must be allowed negative values
	float xIncrement, yIncrement, x, y;

	dx = xb - xa;
	dy = yb - ya;

	x = xa;
	y = ya;

    if(abs(dx) > abs(dy)) steps = abs(dx);
    	else steps = abs(dy);

    xIncrement = dx / (float) steps;
    yIncrement = dy / (float) steps;

    setPixel(ceil(x), ceil(y),gradient);

    for(uint8_t q = 0; q < steps; q++){
		x += xIncrement;
		y += yIncrement;
		setPixel(ceil(x), ceil(y), gradient);
	}

}

/**
*	\brief Draws a quarter circle on the display
*
*	Updates the frameBuffer through setPixel()
*
*	\param uint8_t x0 horizontal center position (0-24)
*	\param uint8_t y0 vertical center position (0-24)
*	\param uint8_t radius radius (0-24)
*	\param uint8_t gradient pixel color (0-15)
*/
void soundWave(uint8_t x0, uint8_t y0, uint8_t radius, uint8_t gradient)
{
  int8_t f = 1 - radius;
  int8_t ddF_x = 1;
  int8_t ddF_y = -2 * radius;
  int8_t x = 0;
  int8_t y = radius;

  setPixel(x0, y0 - radius, gradient);
  setPixel(x0 + radius, y0, gradient);

  while(x < y)
  {
    // ddF_x == 2 * x + 1;
    // ddF_y == -2 * y;
    // f == x*x + y*y - radius*radius + 2*x - y + 1;
    if(f >= 0)
    {
      y--;
      ddF_y += 2;
      f += ddF_y;
    }
    x++;
    ddF_x += 2;
    f += ddF_x;
    //setPixel(x0 + x, y0 + y, gradient);
    setPixel(x0 + x, y0 - y, gradient);
    //setPixel(x0 + y, y0 + x, gradient);
    setPixel(x0 + y, y0 - x, gradient);
  }
}

////////////////////////////////////////////////////////////////////////////////////////

/**
*	\brief Gets a single byte from I2C and updates state accordingly
*
*	Gets a single byte from I2C and updates state accordingly. Once a full 0xdeadbeef1
*	header has been received, checks if next byte is 1 (execute local command) or 0, in
*	which case we simply reads a screenful (325B) over I2C and displays the content
*	on screen.
*
*	This function never returns from its while(1) loop, but calls getTwiByte() to get
*	updated I2C data and calls animation functions as required, who all return here.
*
*/

void serviceInputData(void)
{
	uint8_t *ptr = frameBuffer;
	uint8_t state = 0;
	int counter = 0;
	while (1)
	{
		uint8_t c = getTwiByte();

		// very simple state machine to look for 6 byte start of frame
		// marker and copy bytes that follow into buffer
		if (state  <6)	// state < 6 means we are not in the process of receiving frameBuffer data from MegaMan
		{
         	/*
         	 *	We examine the byte we get from I2C, looking to see if we're building up a 0xdeadbeef1? header
         	 *	Once we have 0xdeadbeef1 (we're then in state 5) we look at the following byte. If it is 1, we
         	 *	read one more byte and execute the corresponding built-in graphical function with runCommand(),
         	 *	if *not*, we get ready run display a 325B frameBuffer that follows from I2C, i.e. then we are in
         	 *	"slave" mode, simply displaying whatever MegaMan sends us, until we have displayed one full
         	 *	frameBuffer and we start looking for a new header again
         	 */

			if      (state == 0 && c == 0xde) state++;	// switching state to 1
			else if (state == 1 && c == 0xad) state++;	// switching state to 2
			else if (state == 2 && c == 0xbe) state++;	// switching state to 3
			else if (state == 3 && c == 0xef) state++;	// switching state to 4
			else if (state == 4 && c == 0x01) state++;	// switching state to 5 (from the next byte we determine what to do with the data)

      		else if (state == 5 && c == 0x01) {			// state is 5 AND we have a command header (0xdeadbeef11)
      			runCommand(getTwiByte());				// sending the next byte, after de-ad-be-ef-01-01, to runCommand();
      			state = 0; 								// command executed, resetting state and starting over

      		} else if (state == 5 ) { 					// state is 5, but NO command header (0xdeadbeef10)
				state++; 								// switching to state 6
				counter = 0;							// preparing the byte counter for transmission of 325B frameBuffer
				ptr = frameBuffer;						// pointing ptr to beginning of Peggy's frameBuffer
			} else state = 0; 							// error: deviation from 0xdeadbeef1?, reset and start over

		} else {
			// inside of a frame, so save each byte to buffer - this is "slave mode" where we blindly display what MegaMan sends us
			*ptr++ = c;	// insert most recently read byte from I2C into frameBuffer on Peggy
			counter++;
			if (counter >= DISP_BUFFER_SIZE) {
				// buffer filled, so reset everything to wait for next frame
				counter = 0;
				ptr = frameBuffer;
				state = 0;
			}
		}
	}	// eternal while loop - never ends
}


/**
*	\brief Main function - initialized the display, I2C, serial and pull-ups. Starts the
*	never returning serviceInputData() function
*/
int main(void)
{

	// Enable pullups for buttons/i2c
	PORTB |= _BV(0);
	PORTC = _BV(5) |  _BV(4) |  _BV(3) | _BV(2) |  _BV(1) |  _BV(0);

	UCSR0B = 0; // turn OFF serial RX/TX, necessary when using Arduino bootloader

	displayInit();

 	initTwiSlave(TWI_SLAVE_ID);

 	sei();

	// clear display and set to test pattern
	// pattern should look just like the "gray test pattern" from EMS
	uint8_t v = 0;
	for (int i =0; i < DISP_BUFFER_SIZE; i++)
	{
		v = (v+2) % 16;
	    // set to 0 for blank startup display
		// low order bits on the left, high order bits on the right (OJ: didn't I flip this one? verify!)
			frameBuffer[i]= v + ((v+1)<<4);
		//	frameBuffer[i]=0;
	}
	serviceInputData();  // never returns
}


///}

